#include <btl/extra/VideoSource/VideoSourceUEye.hpp>
#include <iostream>

namespace btl
{
namespace extra
{
namespace videosource
{

VideoSourceUEye::VideoSourceUEye(const char* iniFile, int deviceIndex, int numBuffers) :
   _deviceIndex(deviceIndex),
   _numBuffers(numBuffers)
{
   _frameSize = Eigen::Vector2i(752, 480);
   _hCam = (HIDS)_deviceIndex;
   is_InitCamera(&_hCam, NULL);
   is_GetCameraInfo(_hCam, &_camInfo);
   is_GetSensorInfo(_hCam, &_sensorInfo);
   is_SetImageSize(_hCam, _frameSize(0), _frameSize(1));
   if(strlen(iniFile) != 0) 
   {
      if(is_LoadParameters(_hCam, iniFile) != IS_SUCCESS)
      {
         std::stringstream ss;
         ss << "Could not load parameter file \"" << iniFile << "\".";
         throw VideoSourceUEye::Exception(ss.str());
      }
   }
   else
   {
      double enable = 1.0;
      is_SetAutoParameter(_hCam, IS_SET_ENABLE_AUTO_SHUTTER, &enable, 0);
      is_SetAutoParameter(_hCam, IS_SET_ENABLE_AUTO_FRAMERATE, &enable, 0);
      is_SetAutoParameter(_hCam, IS_SET_ENABLE_AUTO_WHITEBALANCE, &enable, 0);
   }
   is_EnableEvent(_hCam, IS_SET_EVENT_FRAME);
   _frameBuffers = new FrameBuffer[_numBuffers];
   int numBitsPerPixel = 3 * 8;
   int numBitsPerImage = _frameSize(0) * _frameSize(1) * numBitsPerPixel;
   for(int i = 0; i < _numBuffers; ++i)
   {
      is_AllocImageMem(_hCam, _frameSize(0), _frameSize(1), numBitsPerPixel, &_frameBuffers[i].pData, &_frameBuffers[i].id);
      is_AddToSequence(_hCam, _frameBuffers[i].pData, _frameBuffers[i].id);
      _frameBuffers[i].seqNum = i + 1;
      _frameBuffers[i].size = numBitsPerImage;
   }
   is_CaptureVideo(_hCam, IS_WAIT);
}

VideoSourceUEye::~VideoSourceUEye()
{
   is_StopLiveVideo(_hCam, IS_WAIT);
   for(int i = 0; i < _numBuffers; ++i)
      is_FreeImageMem(_hCam, _frameBuffers[i].pData, _frameBuffers[i].id);
   delete[] _frameBuffers;
   is_DisableEvent(_hCam, IS_SET_EVENT_FRAME);
   is_ExitCamera(_hCam);
}

const ImageRegionConstRGB VideoSourceUEye::getNextFrame()
{
   char *pMem, *pMemLast;
   INT mem_id;
   _frame = ImageRGB(_frameSize(0), _frameSize(1));
   is_GetActSeqBuf(_hCam, &mem_id, &pMem, &pMemLast);
   is_LockSeqBuf(_hCam, IS_IGNORE_PARAMETER, pMemLast);
   char* pb = pMemLast;
   for(int y = 0; y < _frameSize(1); ++y)
   {
      for(int x = 0; x < _frameSize(0); ++x)
      {
         PixelRGB pixel;
         pixel.blue() = *(pb++);
         pixel.green() = *(pb++);
         pixel.red() = *(pb++);
         _frame[y][x] = pixel;
      }
   }
   is_UnlockSeqBuf(_hCam, IS_IGNORE_PARAMETER, pMemLast);
   return ImageRegionConstRGB(_frame);
}

} //namespace videosource
} //namespace extra
} //namespace btl
